// Copyright 2012 Sergio, Raul, Dorival. All rights reserved.
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file.

#ifndef GEMLAB_ALGOS_H
#define GEMLAB_ALGOS_H

#include <cmath>

// Boost Geometry
#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
namespace BOGE = boost::geometry;
BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(BOGE::cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(BOGE::cs::cartesian)
typedef BOGE::model::d2::point_xy<double>    BOGEpoint2D;
typedef BOGE::model::linestring<BOGEpoint2D> BOGElinestring;
typedef BOGE::model::polygon<BOGEpoint2D>    BOGEpolygon2D;

namespace GemLab {

bool operator== (BOGEpoint2D const & a, BOGEpoint2D const & b)
{
    const double POINT_EQUAL_TOL = 1e-4;
    if ( (fabs(a.x() - b.x()) < POINT_EQUAL_TOL) &&
         (fabs(a.y() - b.y()) < POINT_EQUAL_TOL) ) return true;
    return false;
}

bool IsEqual(Vec_t const & a, Vec_t const & b, double Tol=1e-4)
{
    for (int i=0; i<a.rows(); ++i) {
        if (std::fabs(a[i] - b[i]) > Tol) return false;
    }
    return true;
}

// CellFlocal returns a fuction of (r,s,t) that is positive within the element and negative outside
inline double CellFlocal(Vec_t const & R, Cell const *c)
{
    double r=R(0), s=R(1), t=(R.rows()>2 ? R(2) : 0);
    int bgeo = Geo2geob[c->Geo]; // fundamental geometry of cell
    if (bgeo==TRI3) return 27.0*r*s*(1.0-r-s);
    if (bgeo==QUA4) return (1.0-r*r)*(1.0-s*s);
    if (bgeo=TET4)  return 256.0*r*s*t*(1.0-r-s-t);
    if (bgeo=HEX8)  return (1.0-r*r)*(1.0-s*s)*(1.0-t*t);
    throw new Fatal("GemLab::Mesh.CellFlocal cannot handle bgeo=%d", bgeo);
}

inline int CellFalpha(double alpha, Vec_t const & u0, Vec_t const & uu, Cell const *c)
{
    Vec_t u(u0.rows());
    u = u0 + alpha*uu;
    return CellFlocal(u, c);
}

// returns number of intersections, excluding initial point on boundary
inline int CellIntersectLocal(Vec_t const & rst, Vec_t const & drst, Vec_t & irst, Cell const *c)
{
    BOGEpolygon2D p;
    int bgeo = Geo2geob[c->Geo]; // fundamental geometry of cell
    if (bgeo==TRI3)
    {
        double points[][2] = {{0.0, 0.0}, {0.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}};
        BOGE::append(p, points);
    }
    else if (bgeo==QUA4)
    {
        //double points[][2] = {{-1.0, -1.0}, {1.0, -1.0}, {1.0, 1.0}, {-1.0, 1.0}, {-1.0, -1.0}}; // negative area
        double points[][2] = {{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}};
        BOGE::append(p, points);
    }
    else if (bgeo=TET4)
    {
    }
    else if (bgeo=HEX8)
    {
    }
    else throw new Fatal("GemLab::Mesh.CellIntersectLocal cannot handle bgeo=%d", bgeo);

    double points[][2] = {{rst[0],rst[1]}, {rst[0]+drst[0],rst[1]+drst[1]}};
    std::vector<BOGEpoint2D> res;
    BOGElinestring ray;
    BOGE::append(ray, points);
    BOGE::intersection(p, ray, res);

    if (true) { // debugging
        std::cout << "area = " << BOGE::area(p) << std::endl;
        std::cout << "ray = [";
        BOOST_FOREACH(BOGEpoint2D const & pts, ray)
        { std::cout << "(" << pts.x() << ", " << pts.y() << "), "; }
        std::cout << "]\n";
        std::cout << "intersection points = [";
        BOOST_FOREACH(BOGEpoint2D const & pts, res)
        { std::cout << "(" << pts.x() << ", " << pts.y() << "), "; }
        std::cout << "]\n";
    }

    // exclude initial point on boundary
    if (res.size() == 0) {
        irst << ray[1].x(), ray[1].y(), 0.; // tip of ray
        return 0;
    } else if (res.size() == 1) {
        irst << res[0].x(), res[0].y(), 0.;
        if (res[0] == ray[0]) {
            return 0;
        } else {
            return 1;
        }
    } else if (res.size() == 2) {
        if (res[0] == ray[0]) {
            if (res[1] == ray[1]) {
                throw new Fatal("GemLab::Mesh.CellIntersectLocal boost::geometry::intersection returned both results equal to initial point");
            }
            irst << res[1].x(), res[1].y(), 0.;
            return 1;
        }
        if (res[1] == ray[1]) {
            if (res[0] == ray[0]) {
                throw new Fatal("GemLab::Mesh.CellIntersectLocal boost::geometry::intersection returned both results equal to initial point");
            }
            irst << res[0].x(), res[0].y(), 0.;
            return 1;
        }
    }
    else throw new Fatal("GemLab::Mesh.CellIntersectLocal boost::geometry::intersection returned more than 2 results");
}

inline int CellIntersect(Vec_t const & x0, Vec_t const & dx, Vec_t & xi, Cell const *c)
{
}

// Methods for the enriched mesh
void CellInvMap(Cell const * c, Vec_t const & X, Vec_t & R, double Tol=1.0e-7, int MaxIt=25)
{
    int geo = c->Geo;
    int nvc = Geo2nvc[geo];
    int gnd = Geo2gnd[geo];

    Mat_t C(gnd, nvc);
    for (int i=0; i<gnd; ++i)
    for (int j=0; j<nvc;  ++j)
        C(i,j) = c->V[j]->C[i];

    Vec_t res(gnd);       // residual
    Vec_t S(nvc);         // shape functions
    Mat_t dSdR(nvc, gnd); // derivative of S w.r.t natural coords
    Mat_t J(gnd, gnd);    // Jacobian
    Mat_t Ji(gnd, gnd);   // Jacobian
    Vec_t dR(gnd);        // corrector
    R = 0.0 * X;          // trial
    int it;
    double r, s, t;
    for (it=0; it<MaxIt; ++it) {
        r=R(0);  s=R(1);  t=(R.rows()>2 ? R(2) : 0);
        Geo2func[geo](S, dSdR, r, s, t, true);
        res = X - C * S;
        J   = C * dSdR;
        Ji  = J.inverse();
        dR  = Ji * res;
        R  += dR;
        if (dR.norm() < Tol) break;
    }
    if (it==MaxIt) throw new Fatal("Generate::CellInvMap: iterations did not converge after %d iterations", it);
}

// CellDist returns the shortest distance between R and the boundary of the cell in natural coordinates
double CellBryDist(Cell const * c, Vec_t const & R)
{
    double r=R(0), s=R(1), t=(R.rows()>2 ? R(2) : 0);
    int bgeo = Geo2geob[c->Geo]; // fundamental geometry of cell

    if (bgeo==TRI3)
        return std::min(r, std::min(s, 1.0-r-s));

    if (bgeo==QUA4)
        return std::min(1.0 - std::abs(r), 1.0 - std::abs(s));

    if (bgeo==HEX8)
        return std::min(1.0 - std::abs(r), std::min(1.0 - std::abs(s), 1.0 - std::abs(t)));

    if (bgeo==TET4)
        return std::min(r, std::min(s, std::min(t, 1.0-r-s-t)));

    return -1.0; // as outside the cell by default
}

// IsInside indicates whether X is inside a Cell or not
bool IsInside(Cell const * c, Vec_t const & X, double Tol=1.0e-7)
{
    int geo  = c->Geo;
    int ndim = Geo2gnd[geo];
    if (ndim==1) return false;
    Vec_t R(X.rows());
    CellInvMap(c, X, R);
    return (CellBryDist(c, R) > -Tol);
}

// find the cell that contains a given point
// TODO: optimise search algorithm here, e.g.: with a localisation background grid
Cell const * FindCell(Mesh const * m, Vec_t const & X)
{
    for (int i=0; i<m->Cells.Size(); i++)
    {
        //std::cout << "findcell i=" << i << std::endl;
        if (IsInside(m->Cells[i], X)) return m->Cells[i];
    }
    return NULL;
}

}; // namespace GemLab

#endif // GEMLAB_ALGOS_H
